#include <cstdlib>
#include <cstdio>
#include <cctype> 
#include <cassert>


#include "Vector.h"
#include "FEMModel.h"
#include "Node.h"
#include "Triangle.h"
#include "Tetrahedron.h"
#include "BinSearch.h"
#include "ModelIO.h"
#include "Setting.h"
#include "TopologicalOperation.h"
#include "DeformationState.h"

#include "BigVector.h"

char * InFileRead (FILE *f, int len ,char *s)
{
	char * p =0; 
	while ((p = fgets (s,len, f)))
	{
		while (*p && isspace (*p))
			p++;

		if (*p && *p != '#')
			break;
	}

	return p;
}

Vector3 ReadVector (FILE *f)
{
	const int BUFLEN = 2048;
	char s[BUFLEN];
	InFileRead (f, BUFLEN, s);

	Vector3 v;
	int c = sscanf (s, "%lf %lf %lf", &v(0), &v(1), &v(2));
	assert (c == 3);
	return v;
}





void WriteVector (FILE *f, Vector3 v)
{
	fprintf (f, "%f %f %f\n", v(0),v(1),v(2));
}

FiniteElementModel *ReadFEM (char const *fn)
{
	FiniteElementModel *fem = new FiniteElementModel;

	FILE * f = fopen (fn, "r");
	if (!f)
	{
		fprintf(stderr, "oops. Can't read %s\n", fn);
		exit (1);
	}

	fprintf (stderr, "Reading `%s' ... ", fn);
	fflush (stderr);

	const int BUFLEN = 2048;
	char s[BUFLEN];
	InFileRead (f, BUFLEN, s);

	int nod_count;
	sscanf (s, "%d", &nod_count);

	for (int i=nod_count; i--;)
	{
		Vector3 v = ReadVector (f);

		fem->MakeNode (v);
	}

	int tet_count;
	InFileRead (f, BUFLEN, s);
	sscanf (s, "%d", &tet_count);


	LinkArray<Node> failed;
	for (int i = tet_count; i--; )
	{
		Node *ns [4] = {0,0,0,0};
		char * end  = InFileRead (f, BUFLEN, s);

		if (!end)
		{
			fprintf(stderr, "Early stop: %d tets to go" , i+1);
			break;
		}
		for (int j=0; j < 4; j++)
		{
			int c  = strtol (end, &end, 10);

			if (c >= fem->NodeCount ())
			{
				fprintf (stderr, "Found invalid node index %d\n", c);
				exit (2);
			}

			ns[j]=  fem->GetNode (c);
		}

		int p;
		int succes = sscanf (end, "%d", &p);
		if (succes == 1)
		{
			fem->MakeTetrahedron (Simplex (TETRAHEDRON, ns, p));
		}
		else
		{
			Tetrahedron * succ = MakePositiveTetrahedron (fem, ns);

			if (!succ)
				for (int k =0 ; k < 4; k++)
					failed.Push (ns[k]);
		}
	}

	/*
	Do a desparate attempt to put in degenerate tetra anyway.
	*/
	bool failed_found = failed.Size ();
	if (failed_found && GetNumberSetting ("0-volume-read"))
	{
		int lastsiz = -1;

		printf("Trying to accomodate failed tetras\n");

		while (failed.Size () != lastsiz)
		{
			LinkArray<Node> newfail;
			int i= failed.Size () -1;
			while (i > 0)
			{
				Node * ns[4];
				for (int k = 0; k < 4; k++)
					ns[k] = failed[i--];

				bool succ = false;
				for  (int p = 0; !succ && p  <1 ; p++)
				{
					Simplex ts (TETRAHEDRON, ns, p);
					for (int j = 0; !succ &&  j < 4; j++)
						if (fem->FindObject (ts.GetSubset (j).GetMate ()))
						{
							succ = true;
							fem->MakeTetrahedron (ts);
						}
				}

				if (!succ)
					for (int k = 4; k--;)
						newfail.Push (ns[k]);
			}
			lastsiz = failed.Size ();
			failed = newfail;
		}


		if (failed_found)
			CollapseDegeneracies (fem);
	}
	fclose (f);
	fprintf (stderr, "done\n");  

	/*
	clumsy for debugging 
	*/
	if (!strcmp (GetStringSetting ("normalize-input"), "yes"))
		fem->CenterModel ();

	MakeCutRegular (fem);

	fem->Validate ();

	return fem;
}


int TetCompare (Tetrahedron* const &p1, Tetrahedron* const &p2)
{
	return p1->Number () - p2->Number ();
}

void WriteFEM (FiniteElementModel const * fem,char const *fn)
{
	LinkArray<Tetrahedron> tets (fem->TetrahedronArray ());
	WriteFEMSubset (&tets, fn);
}


int NodeCompare (Node * const &n1,Node * const &n2)
{
	return n1->Number () - n2->Number ();
}
/*
Write the subset in TETS to a file FN. The file will have renumbered
nodes, but the ordering of the nodes and tetrahedra is maintained.

RETURN

the set of nodes, ordered by  Node::number

SIDE EFFECT

TETS is sorted according to Topological_object::number

*/

LinkArray<Node> WriteFEMSubset (LinkArray<Tetrahedron> *tets, const char *fn)
{
	tets->Sort (Tetrahedron::Compare);
	tets->Uniq ();

	FILE * f = fopen (fn, "w");
	if (!f)
	{
		fprintf(stderr, "oops. Can't write %s\n", fn);
		exit (1);
	}

	fprintf (stderr, "Writing `%s' ... ", fn);
	fflush (stderr);



	map<Node*, int> nodindex;
	LinkArray<Node> nods;

	for (int i = tets->Size (); i--;)
		for (int j = 4; j--;)
			nods.Push (tets->Elem (i)->GetNode(j)); 

	nods.Sort (&NodeCompare);
	nods.Uniq ();
	for (int i = nods.Size ();  i--;)
		nodindex [nods[i]] = i;


	fprintf (f, "%d\n", nods.Size ());
	for (int i = 0; i < nods.Size (); i++)
		WriteVector (f, nods[i]->Location (false));

	fprintf (f, "%d\n", tets->Size ());
	for (int i = 0; i < tets->Size (); i++)
	{
		Simplex s= tets->Elem (i)->GetSet ();
		for (int k =0; k < 4; k++)
			fprintf (f, "%5d ", nodindex[s.GetNode (k)]);

		fprintf (f,  " %d ", s.Parity( ));
		fprintf (f,"\n");

	}
	fclose (f);
	fprintf (stderr, "done\n");

	return nods;
}

void WriteBigVector (FILE * f,  Real const *a , int n)
{
	while (n--)
	{
		fprintf (f, "%lf " , *a++);
	}
	fputc ('\n', f);
}


void ReadBigVector (FILE* f, Real *a, int n)
{
	while (n--)
	{
		int d =      fscanf (f,"%lf " , a++);
		assert (d == 1);
	}
	fscanf (f,"\n");
}


void WriteOptions (FILE*f)
{
	extern char ** argument_value;
	fputc ('#', f); 
	for (char **a =  argument_value;
		*a; a ++)
	{
		fprintf (f, "%s ", *a);
	}
	fputc ('\n',f);
}


void WriteDeformationState (DeformationState const*pstate, char const *fn)
{
	fprintf (stderr, "Writing `%s' ... \n", fn);

	FILE * f = fopen (fn, "w");
	if (!f)
	{
		fprintf(stderr, "Can't write file..\n");
		return;
	}
	int n =pstate->Dimension ();

	WriteOptions (f);
	fprintf (f, "%d\n",   n);

	WriteBigVector (f, pstate->displacement_.AccessArray (), n);
	WriteBigVector (f, pstate->external_force_.AccessArray (), n);

	bool const * bp = pstate->fixed_arr_.AccessArray ();
	while (n--)
	{
		fprintf (f, "%d " , (int) *bp++);
	}

	fclose (f);
}


/*
Try to read deformation state into PSTATE.

RETURN

whether success


SIDE EFFECT

pstate is modified. Message printed if failed. 
*/

bool ReadDeformationState (DeformationState * pstate, char const * fn)
{
	FILE * f = fopen (fn, "r");
	if (!f)
	{
		fprintf (stderr, "State file `%s' not found. Not reading deformation state.\n", fn);
		return false;
	}
	else
	{
		fprintf (stderr, "Reading `%s' ...\n", fn);
	}

	char ch = fgetc(f);
	if (ch == '#')
	{
		int c;
		while ((c = fgetc (f)) != EOF)
			if (c == '\n' )
				break;
	}
	else
		ungetc(ch, f);

	int n ;
	int succ = fscanf (f, "%d", &n);
	assert (succ == 1);

	if (pstate->fem_->NodeCount ()*3 !=  n)
	{
		fprintf (stderr, "Node counts don't match (want %d, got %d)\n" ,
			pstate->fem_->NodeCount ()*3, n);
		return false;

	}

	pstate->displacement_.SetSize (n);  pstate->external_force_.SetSize (n);
	pstate->fixed_arr_.SetSize (n);
	ReadBigVector (f, pstate->displacement_.UnsafeAccessArray (),n);
	ReadBigVector (f, pstate->external_force_.UnsafeAccessArray (),n);

	bool *bp = pstate->fixed_arr_.UnsafeAccessArray ();
	while (n--)
	{
		int fix ;
		int c =  fscanf (f, "%d", &fix);
		assert (c == 1);
		*bp ++ = fix;
	}
	fclose (f);

	return true;
}


Array<Real> reference_configuration; 

/*
Compare PSTATE to reference state, returning the euclidian distance. 
*/
Real DeformationStateDistance (DeformationState const* pstate)
{
	if (!reference_configuration.Size ())
	{
		char const *fn = GetStringSetting ("reference-state-file");

		DeformationState * read_state = new DeformationState(pstate->fem_);
		bool succ = ReadDeformationState (read_state, fn);

		if (!succ)
			reference_configuration.Push (0.0);
		else
			reference_configuration = read_state->displacement_;
		delete read_state ;
	}

	if (pstate->Dimension () != reference_configuration.Size ())
	{
		return -1.0;
	}
	else
		return BigVectorDistance (reference_configuration.AccessArray (),
		pstate->displacement_.AccessArray (),
		pstate->Dimension ());
}



/*
Write the boundary of MOD (a collection of nodes and a set of triangles.) 
*/
void WriteBoundary (FiniteElementModel*mod, char const *fn)
{
	fprintf (stderr, "Writing boundary to `%s' ...",  fn);
	fflush (stderr);
	FILE * f = fopen (fn, "w");
	if (!f)
	{
		fprintf (stderr, "Error. Can't write file!\n");
		return;
	}

	map<Node*,int>  nods;
	int k = 0;

	set<Triangle*>::const_iterator tbegin = mod->OuterFacesBegin ();
	set<Triangle*>::const_iterator tend = mod->OuterFacesEnd ();  

	for (set<Triangle*>::const_iterator i (tbegin); i != tend; i++)
	{
		for (int j = 0 ; j <3 ; j++)
		{
			Node * tn = (*i)->GetNode (j);
			if (nods.find (tn) == nods.end ())
				nods[tn] = k++;	
		}
	}

	map<int,Node*> revnods;
	for (map<Node*,int>::const_iterator i (nods.begin ()); i != nods.end (); i++)
	{
		revnods[(*i).second] = (*i).first;
	}

	fprintf (f, "%d\n", k);
	for (map<int,Node*>::const_iterator i (revnods.begin ()); i != revnods.end (); i++)
	{
		WriteVector (f, (*i).second->Location (false));
	}

	k = 0;
	for (set<Triangle*>::const_iterator i (tbegin); i != tend; i++)
		k++;

	fprintf (f, "%d\n",k);
	for (set<Triangle*>::const_iterator i (tbegin); i != tend; i++)
	{
		if ((*i)->GetSet ().Parity ())
			for (int j =3;j--;)
				fprintf (f, "%d ", nods [(*i)->GetNode (j)]);
		else
			for (int j =0;j<3;j++)
				fprintf (f, "%d ", nods [(*i)->GetNode (j)]);

		fputs ("\n", f);
	}
	fprintf (stderr, "done\n");

	fclose (f);
}

